#include "CenterOfMassSensorPlot.h"

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace MMM;

CenterOfMassSensorPlot::CenterOfMassSensorPlot(WholeBodyDynamicSensorPtr sensor) :
    sensor(sensor)
{
}

std::vector<std::string> CenterOfMassSensorPlot::getNames() {
    std::vector<std::string> comNames;
    comNames.push_back("CoM_x");
    comNames.push_back("CoM_y");
    comNames.push_back("CoM_z");
    return comNames;
}

std::tuple<QVector<double>, QVector<double> > CenterOfMassSensorPlot::getPlot(std::string name) {
    std::vector<std::string> results;
    boost::algorithm::split(results, name, boost::is_any_of("_"));
    int index = ((int)results[1][0]) - ((int)'x');

    QVector<double> x(sensor->getTimesteps().size()), y(sensor->getTimesteps().size());
    for (unsigned int j = 0; j < sensor->getTimesteps().size(); j++) {
        float timestep = sensor->getTimesteps()[j];
        MMM::WholeBodyDynamicSensorMeasurementPtr measurement = sensor->getDerivedMeasurement(timestep);
        x[j] = timestep;
        y[j] = measurement->getCenterOfMass()(index);
    }
    return std::make_tuple(x, y);
}

std::string CenterOfMassSensorPlot::getSensorName() {
    return sensor->getUniqueName();
}
